/*
 * Copyright (c) 2020 Fraunhofer FOKUS and others. All rights reserved.
 *
 * See the NOTICE file(s) distributed with this work for additional
 * information regarding copyright ownership.
 *
 * This program and the accompanying materials are made available under the
 * terms of the Eclipse Public License 2.0 which is available at
 * http://www.eclipse.org/legal/epl-2.0
 *
 * SPDX-License-Identifier: EPL-2.0
 *
 * Contact: mosaic@fokus.fraunhofer.de
 */
package org.eclipse.mosaic.fed.carla.clojure;


import libpython_clj2.java_api;
import org.apache.commons.lang3.StringUtils;
import org.eclipse.mosaic.lib.geo.CartesianPoint;
import org.eclipse.mosaic.lib.objects.vehicle.VehicleData;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

import java.util.Map;

public abstract class AbstractCarlaCommand {

    private final Logger log = LoggerFactory.getLogger(this.getClass());
    protected static final double SPAWN_OFFSET_Z = 25.0;

    protected AbstractCarlaCommand() {

    }


    /*
         ______   _______ _________ ______   _______  _______
         (  ___ \ (  ____ )\__   __/(  __  \ (  ____ \(  ____ \
         | (   ) )| (    )|   ) (   | (  \  )| (    \/| (    \/
         | (__/ / | (____)|   | |   | |   ) || |      | (__
         |  __ (  |     __)   | |   | |   | || | ____ |  __)
         | (  \ \ | (\ (      | |   | |   ) || | \_  )| (
         | )___) )| ) \ \_____) (___| (__/  )| (___) || (____/\
         |/ \___/ |/   \__/\_______/(______/ (_______)(_______/

         _______  _        _______  _______  _______
         |\     /|(  ____ \( \      (  ____ )(  ____ \(  ____ )
         | )   ( || (    \/| (      | (    )|| (    \/| (    )|
         | (___) || (__    | |      | (____)|| (__    | (____)|
         |  ___  ||  __)   | |      |  _____)|  __)   |     __)
         | (   ) || (      | |      | (      | (      | (\ (
         | )   ( || (____/\| (____/\| )      | (____/\| ) \ \__
         |/     \|(_______/(_______/|/       (_______/|/   \__

         we assume methods below are executed under the condition of
         locking GIL lock, please check condition before using these methods
     */


    protected Object getCarlaBlueprint(VehicleData sumoVehicleData) {

        Map global = java_api.runStringAsFile(
                "\n" +
                        "import random \n" +
                        "import logging \n" +
                        "def _get_recommended_carla_blueprint(v_class):\n" +
                        "        blueprints = []\n" +
                        "        for blueprint in blueprint_library:\n" +
                        "            if blueprint.id in _VTYPES and \\\n" +
                        "               _VTYPES[blueprint.id]['vClass'] == v_class:\n" +
                        "               blueprints.append(blueprint)\n" +
                        "\n" +
                        "        if not blueprints:\n" +
                        "            return None\n" +
                        "\n" +
                        "        return random.choice(blueprints) \n" +
                        "def get_carla_blueprint(type_id, v_class, sync_color=False):\n" +
                        "\n" +
                        "        if type_id in [bp.id for bp in blueprint_library]:\n" +
                        "            blueprint = blueprint_library.filter(type_id)[0]\n" +
                        "        else:\n" +
                        "            blueprint = _get_recommended_carla_blueprint(v_class)\n" +
                        "            if blueprint is not None:\n" +
                        "                logging.warning(\n" +
                        "                    'sumo vtype %s not found in carla. The following blueprint will be used: %s',\n" +
                        "                    type_id, blueprint.id)\n" +
                        "            else:\n" +
                        "                logging.error('sumo vtype %s not supported. No vehicle will be spawned in carla',\n" +
                        "                              type_id)\n" +
                        "                return None\n" +
                        "\n" +
                        "        if blueprint.has_attribute('color'):\n" +
                        "            if sync_color:\n" +
                        "                color = \"{},{},{}\".format(sumo_actor.color[0], sumo_actor.color[1],\n" +
                        "                                          sumo_actor.color[2])\n" +
                        "            else:\n" +
                        "                color = random.choice(blueprint.get_attribute('color').recommended_values)\n" +
                        "            blueprint.set_attribute('color', color)\n" +
                        "\n" +
                        "        if blueprint.has_attribute('driver_id'):\n" +
                        "            driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values)\n" +
                        "            blueprint.set_attribute('driver_id', driver_id)\n" +
                        "\n" +
                        "        blueprint.set_attribute('role_name', 'sumo_driver')\n" +
                        "\n" +
                        "        return blueprint"
        );

        Object getCarlaBlueprint = global.get("get_carla_blueprint");
        return java_api.call(getCarlaBlueprint, sumoVehicleData.getVehicleType().getName(), sumoVehicleData.getVehicleClass());
    }

    protected Object getCarlaTransform(ClojureCarlaConnection connection, VehicleData sumoVehicleData) {
        Object carlaModule = connection.carlaModule;

        // prepare sumo transform, extend
        CartesianPoint position = sumoVehicleData.getProjectedPosition();
        double slope = sumoVehicleData.getSlope();
        double angel = sumoVehicleData.getHeading();
        double length = sumoVehicleData.getVehicleType().getLength();
        double height = sumoVehicleData.getVehicleType().getHeight();
        double width = sumoVehicleData.getVehicleType().getWidth();

        Object locationAttr = java_api.getAttr(carlaModule, "Location");
        Object rotationAttr = java_api.getAttr(carlaModule, "Rotation");
        Object transformAttr = java_api.getAttr(carlaModule, "Transform");
        Object vector3DAttr = java_api.getAttr(carlaModule,  "Vector3D");

        Object location = java_api.call(locationAttr, position.getX(), position.getY(), position.getZ());
        Object rotation = java_api.call(rotationAttr, slope, angel, 0.0);
        Object transform = java_api.call(transformAttr, location, rotation);
        Object extend = java_api.call(vector3DAttr, length / 2.0, width / 2.0, height / 2.0);

        // transform
        Map global = java_api.runStringAsFile(
                "\n" +
                        "import carla \n" +
                        "import math \n" +
                        "def get_carla_transform(in_sumo_transform, extent):\n" +
                        "        in_location = in_sumo_transform.location\n" +
                        "        in_rotation = in_sumo_transform.rotation\n" +
                        "\n" +
                        "        yaw = -1 * in_rotation.yaw + 90\n" +
                        "        pitch = in_rotation.pitch\n" +
                        "        out_location = (in_location.x - math.cos(math.radians(yaw)) * extent.x,\n" +
                        "                        in_location.y - math.sin(math.radians(yaw)) * extent.x,\n" +
                        "                        in_location.z - math.sin(math.radians(pitch)) * extent.x)\n" +
                        "        out_rotation = (in_rotation.pitch, in_rotation.yaw, in_rotation.roll)\n" +
                        "\n" +
                        "        out_location = (out_location[0] - offset[0], out_location[1] - offset[1], out_location[2])\n" +
                        "\n" +
                        "        out_transform = carla.Transform(\n" +
                        "            carla.Location(out_location[0], -out_location[1], out_location[2]),\n" +
                        "            carla.Rotation(out_rotation[0], out_rotation[1] - 90, out_rotation[2]))\n" +
                        "\n" +
                        "        return out_transform"
        );
        Object getCarlaTransform = global.get("get_carla_transform");
        return java_api.call(getCarlaTransform, transform, extend);
    }

    protected Object getCarlaLightsState(Object carlaActor, VehicleData sumoVehicleData) {
        Object getLightState = java_api.getAttr(carlaActor, "get_light_state");
        Object currentCarlaLights = java_api.call(getLightState);
        int sumoLights = sumoVehicleData.getVehicleSignals().getCode();

        Map global = java_api.runStringAsFile(
                "\n" +
                        "class SumoSignalState(object):\n" +
                        "    \"\"\"\n" +
                        "    SumoSignalState contains the different traffic light states.\n" +
                        "    \"\"\"\n" +
                        "    RED = 'r'\n" +
                        "    YELLOW = 'y'\n" +
                        "    GREEN = 'G'\n" +
                        "    GREEN_WITHOUT_PRIORITY = 'g'\n" +
                        "    GREEN_RIGHT_TURN = 's'\n" +
                        "    RED_YELLOW = 'u'\n" +
                        "    OFF_BLINKING = 'o'\n" +
                        "    OFF = 'O'\n" +
                        "\n" +
                        "def get_carla_lights_state(current_carla_lights, sumo_lights):\n" +
                        "        current_lights = current_carla_lights\n" +
                        "\n" +
                        "        # Blinker right / emergency.\n" +
                        "        if (any([\n" +
                        "                bool(sumo_lights & SumoVehSignal.BLINKER_RIGHT),\n" +
                        "                bool(sumo_lights & SumoVehSignal.BLINKER_EMERGENCY)\n" +
                        "        ]) != bool(current_lights & carla.VehicleLightState.RightBlinker)):\n" +
                        "            current_lights ^= carla.VehicleLightState.RightBlinker\n" +
                        "\n" +
                        "        # Blinker left / emergency.\n" +
                        "        if (any([\n" +
                        "                bool(sumo_lights & SumoVehSignal.BLINKER_LEFT),\n" +
                        "                bool(sumo_lights & SumoVehSignal.BLINKER_EMERGENCY)\n" +
                        "        ]) != bool(current_lights & carla.VehicleLightState.LeftBlinker)):\n" +
                        "            current_lights ^= carla.VehicleLightState.LeftBlinker\n" +
                        "\n" +
                        "        # Break.\n" +
                        "        if (bool(sumo_lights & SumoVehSignal.BRAKELIGHT) !=\n" +
                        "                bool(current_lights & carla.VehicleLightState.Brake)):\n" +
                        "            current_lights ^= carla.VehicleLightState.Brake\n" +
                        "\n" +
                        "        # Front (low beam).\n" +
                        "        if (bool(sumo_lights & SumoVehSignal.FRONTLIGHT) !=\n" +
                        "                bool(current_lights & carla.VehicleLightState.LowBeam)):\n" +
                        "            current_lights ^= carla.VehicleLightState.LowBeam\n" +
                        "\n" +
                        "        # Fog.\n" +
                        "        if (bool(sumo_lights & SumoVehSignal.FOGLIGHT) !=\n" +
                        "                bool(current_lights & carla.VehicleLightState.Fog)):\n" +
                        "            current_lights ^= carla.VehicleLightState.Fog\n" +
                        "\n" +
                        "        # High beam.\n" +
                        "        if (bool(sumo_lights & SumoVehSignal.HIGHBEAM) !=\n" +
                        "                bool(current_lights & carla.VehicleLightState.HighBeam)):\n" +
                        "            current_lights ^= carla.VehicleLightState.HighBeam\n" +
                        "\n" +
                        "        # Backdrive (reverse).\n" +
                        "        if (bool(sumo_lights & SumoVehSignal.BACKDRIVE) !=\n" +
                        "                bool(current_lights & carla.VehicleLightState.Reverse)):\n" +
                        "            current_lights ^= carla.VehicleLightState.Reverse\n" +
                        "\n" +
                        "        # Door open left/right.\n" +
                        "        if (any([\n" +
                        "                bool(sumo_lights & SumoVehSignal.DOOR_OPEN_LEFT),\n" +
                        "                bool(sumo_lights & SumoVehSignal.DOOR_OPEN_RIGHT)\n" +
                        "        ]) != bool(current_lights & carla.VehicleLightState.Position)):\n" +
                        "            current_lights ^= carla.VehicleLightState.Position\n" +
                        "\n" +
                        "        return current_lights"
        );
        Object getCarlaLightsState = global.get("get_carla_lights_state");
        return java_api.call(getCarlaLightsState, currentCarlaLights, sumoLights);
    }

    protected long spawnerCarlaActor(ClojureCarlaConnection connection, Object carlaBlueprint, Object carlaTransform) {
        Object client = connection.carlaClient;

        java_api.setGlobal("SPAWN_OFFSET_Z", SPAWN_OFFSET_Z);
        Map global = java_api.runStringAsFile(
                "\n" +
                        "import carla \n" +
                        "def get_batch(blueprint, transform):\n" +
                        "       transform = carla.Transform(transform.location + carla.Location(0, 0, SPAWN_OFFSET_Z),\n" +
                        "                                    transform.rotation)\n" +
                        "\n" +
                        "       batch = [\n" +
                        "            carla.command.SpawnActor(blueprint, transform).then(\n" +
                        "                carla.command.SetSimulatePhysics(carla.command.FutureActor, False))\n" +
                        "        ]\n" +
                        "       return batch"
        );
        Object getBatchCarlaActor = global.get("get_batch");
        Object batch = java_api.call(getBatchCarlaActor, carlaBlueprint, carlaTransform);

        Object applyBatchSync = java_api.getAttr(client, "apply_batch_sync");
        Object responses = java_api.call(applyBatchSync, batch, Boolean.FALSE);
        Object response = java_api.getItem(responses, 0);

        Object errorPy = java_api.getAttr(response, "error");
        Object actorIdPy = java_api.getAttr(response, "actor_id");
        String error = (String) java_api.copyToJVM(errorPy);
        long actorId = (long) java_api.copyToJVM(actorIdPy);

        if (!StringUtils.isBlank(error)) {
            log.error("Spawn carla actor failed. {}", error);
            throw new RuntimeException(error);
        }

        return actorId;
    }

    protected Object getCarlaActor(ClojureCarlaConnection connection, long actorId) {
        Object carlaWorld = connection.carlaWorld;
        Object getActorAttr = java_api.getAttr(carlaWorld, "get_actor");
        return java_api.call(getActorAttr, actorId);
    }

    protected Object getCarlaActors(ClojureCarlaConnection connection) {
        Object carlaWorld = connection.carlaWorld;
        Object getActorsAttr = java_api.getAttr(carlaWorld, "get_actors");
        return java_api.call(getActorsAttr);
    }
}
